Representing State with Matrices

The State Vector

You just learned how to represent a self-driving car's state using a motion model. It turns out that matrices provide a very convenient and compact form for representing a vehicle's state.

Let's go back to the constant velocity motion model:

distance = velocity \times time

The vehicle's state is represented by the two variables distance and velocity. If you were going to store these two variables in Python, you'd probably use a list like this:

state = [distance, velocity]

That Python code looks a lot like a mathematical concept called a vector. A vector is essentially a list where each element in the list contains some information.

You could imagine that a state vector could have even more information. In a two-dimensional world, the state could have a distance_x, distance_y, velocity_x and a velocity_y.

In Python, the list would look like this:

state = [distance_x, distance_y, velocity_x, velocity_y]

Or an even more complex model could include information about the turning angle of the vehicle and the turning rate:

state = [distance_x, distance_y, velocity_x, velocity_y, angle, angle_rate]

State Vector in a One-Dimensional World

For now, consider the one-dimensional model with just distance and velocity.

state = [distance, velocity]

How did you calculate the distance and velocity of the vehicle over time when the velocity was constant? There were two different equations.

\begin{cases}distance = velocity \times time \\velocity = velocity\end{cases}

Now, think about these equations in terms of state. For convenience, you can represent distance as x, velocity as v, and time as t.

Initial State

When the vehicle first starts moving, you can consider that t = t_0, x = x_0 and v = v_0. So the state vector is state_0 = [x_0, v_0].

First Time Step

What about after a certain amount of time has passed and now you are at a time t_1?

At t_1, the state vector is:

state_1 = [x_1, v_1]

According to the model formulas,

x_1 = x_0 + v_0 \times (t_1 - t_0)

and since velocity is constant:

v_1 = v_0.

Second Time Step

Then after the next time step t_2, the state vector is:
state_2 = [x_2, v_2]

where

x_2 = x_1 + v_0 \times (t_2 - t_1)

and since velocity is constant
v_2 = v_0.

A Better Way

The math so far is not too hard, right? You have a distance equation and a velocity equation. You plug in the previous state into each equation, the time lapse, and you get the new velocity and the new distance.

But imagine what will happen as your self-driving car model gets more complex. What happens when you have to take into account an x-direction, y-direction, x and y velocities, and steering angle and angular velocity? Or what about an even more complex model like a drone or helicopter that also has a z-direction?

Instead of updating your equations one by one, you can actually use vectors and matrices to do all of the calculations in just one step.

Updating State with Matrix Algebra

If you look back at the equation that updates the distance, you'll notice that distance depends on the previous distance, the initial velocity, and how much time has elapsed since the distance formula was updated. You end up with a generic function:

x_{t+1} = x_{t} + v_0 \times (t_{t+1} - t_{t})

You can also write t_{t+1} - t_{t} as:

\Delta t

For a constant velocity model, the generic velocity equation becomes:
v_{t+1} = v_{t}

How could you combine the x and v equations into one matrix algebra expression? The matrix algebra would look like this:

\begin {bmatrix} x_{t+1} \\ v_{t+1} \end {bmatrix} =\begin {bmatrix} 1& \Delta t \\0 & 1 \end{bmatrix}\times\begin {bmatrix} x_{t} \\ v_{t} \end{bmatrix}

Don't worry if you're not sure what this expression means or how to multiply these matrices. You will learn how in this lesson.

Notation

The matrix algebra equation you just saw is actually one part of the Kalman filter update equation.

Traditionally, the matrix operation:

\begin {bmatrix} x_{t+1} \\ v_{t+1} \end {bmatrix} =\begin {bmatrix} 1& \Delta t \\0 & 1 \end{bmatrix}\times\begin {bmatrix} x_{t} \\ v_{t} \end{bmatrix}

is represented by this notation for Kalman filters:
\mathbf{\hat{x}{k|k-1}} = \mathbf{F} \mathbf{\hat{x}{k-1|k-1}}

where
\mathbf{\hat{x}} is the state vector and \mathbf{F} is the matrix

\begin {bmatrix} 1& \Delta t \\0 & 1 \end{bmatrix}

\mathbf{k - 1} is the previous step and \mathbf{k } is the current step.

You will see in the next part of the lesson why the notation contains \mathbf{k-1|k-1} and \mathbf{k|k-1} .

This notation can get a little bit confusing. For example, what is the difference between
x and \mathbf{\hat{x}}?

The regular x would usually represent distance along the x-axis; on the other hand, the bold \mathbf{\hat{x}} indicates a vector. In the one-dimensional case being discussed here, the \mathbf{\hat{x}} vector contains two variables: distance along the x-axis and velocity; hence
\bold{\hat{x}} = \begin{bmatrix} x \\ v \end{bmatrix}.

Why is there a capitalized bold \mathbf{F} instead of \mathbf{f}? The capitalized, bold \mathbf{F} tells you that this variable is a matrix.

Match the variable with its type

QUIZ QUESTION::

Match the variable with its type

ANSWER CHOICES:

Variable	Type
scalar
matrix
scalar
matrix
vector
scalar
vector

SOLUTION:

Variable	Type
scalar
scalar
scalar
matrix
matrix
scalar
scalar
scalar
matrix
matrix
vector
vector
scalar
scalar
scalar
vector
vector